/*********************************************************************
 * Copyright (C) 2013 Bert Havinga
 * Inspired by the Railuino idea of Joerg Pleumann
 * 
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * LICENSE file for more details.
 */

// #include <can.h>
#include "Railuino.h"
#include "TimerOne/TimerOne.h"
#include "TimerOne/TimerOne.cpp"

size_t printHex(Print &p, unsigned long hex, int digits) {
    size_t size = 0;

    String s = String(hex, HEX);

    for (int i = s.length(); i < digits; i++) {
        size += p.print("0");
    }

    size += p.print(s);

    return size;
}

int parseHex(String &s, int start, int end, boolean *ok) {
    int value = 0;

    for (int i = start; i < end; i++) {
    	char c = s.charAt(i);

        if (c >= '0' && c <= '9') {
            value = 16 * value + c - '0';
        } else if (c >= 'a' && c <= 'f') {
            value = 16 * value + 10 + c - 'a';
        } else if (c >= 'A' && c <= 'F') {
            value = 16 * value + 10 + c - 'A';
        } else {
        	ok = false;
            return -1;
        }
    }

    return value;
}
	/**
	 * Stores the debug flag. When debugging is on, all outgoing and
	 * incoming messages are printed to the Serial console.
	 */
volatile  boolean mS88Debug;


#define SIZE 32

#define ulong unsigned long


//volatile int mSize;

  /**
   * The most recent contact values we know.
   */ 
//  volatile byte mSwitches[MAX_REPORTER];

  /**
   * The previous contact values we know.
   */ 
//  volatile byte mPrevSwitches[MAX_REPORTER];





// ===================================================================
// === TrackMessage ==================================================
// ===================================================================

void TrackMessage::clear() {
	prio = 0;
  command = 0;
	hash = 0;
	response = false;
	length = 0;
	for (int i = 0; i < 8; i++) {
		data[i] = 0;
	}
}

size_t TrackMessage::printTo(Print& p) const {
    size_t size = 0;

    size += printHex(p, hash, 4);
    size += p.print(response ? " R " : "   ");
    size += printHex(p, command, 2);
    size += p.print(" ");
    size += printHex(p, length, 1);

    for (int i = 0; i < length; i++) {
        size += p.print(" ");
        size += printHex(p, data[i], 2);
    }

    return size;
}

boolean TrackMessage::parseFrom(String &s) {
	boolean result = true;

	clear();

	if (s.length() < 11) {
		return false;
	}
// message conforms to CAN bus message PP CC HHHH R L DD DD DD DD DD DD DD DD
  prio = parseHex(s, 0, 2, &result);
	command = parseHex(s, 3, 5, &result);
	hash = parseHex(s, 6, 10, &result);
	response = s.charAt(11) != ' ';
	length = parseHex(s, 13, 15, &result);

	if (length > 8) {
		return false;
	}

	if (s.length() < 11 + 3 * length) {
		return false;
	}

	for (int i = 0; i < length; i++) {
		data[i] = parseHex(s, 12 + 3 * i, 12 + 3 * i + 2, &result);
	}

	return result;
}

void TrackMessage::printmessage() {
			Serial.print("ID :");
			Serial.println(hash, HEX);
			Serial.print("CMD :");
			Serial.println(command, HEX);

			Serial.print("RSP:");
			Serial.println(response, HEX);
			Serial.print("DLC:");
			Serial.println(length, HEX);
			Serial.print("DATA:");

			for (int i = 0; i < length; i++) {
				Serial.print(" ");
        Serial.print(data[i], HEX);
			}
  		Serial.println();
}


void TrackMessage::sendmessage() {
  tx_msg[0] = ((command >> 7) & 0x1F);  // save prio and Command msbit
  tx_msg[1] = ((command  & 0x7F) << 1);
  tx_msg[1] |= (response & 0x01);
  tx_msg[2] = ((hash >> 8) & 0x00ff);
  tx_msg[3] = (hash & 0x00ff);
  tx_msg[4] = length;
  for (int i = 0; i < length; i++) {
    tx_msg[i+5] = data[i];
  }
  for (int i=8; i> length; i--) {
    tx_msg[i+4] = 0;
  }  
    Serial.write(tx_msg, 13);
}


// ===================================================================
// === TrackReporterS88 ==============================================
// ===================================================================




TrackReporterS88::TrackReporterS88(boolean debug) {
	mS88Debug = debug;
	pinMode(DATA, INPUT);
	pinMode(CLOCK, OUTPUT);	//define ports
	pinMode(LOAD, OUTPUT);
	pinMode(RESET, OUTPUT);
	pinMode(LED, OUTPUT);
}

void TrackReporterS88::readreporters(int nr_of_rep) {
	int myByte = 0;
	int myBit = 0;

	//initialize port values
	digitalWrite(CLOCK, LOW);
	digitalWrite(LOAD, LOW);
	digitalWrite(RESET, LOW);

	// take the status of all switches
	digitalWrite(LOAD, HIGH);
	delayMicroseconds( TIME);
	digitalWrite(CLOCK, HIGH);
	delayMicroseconds(TIME);
	digitalWrite(CLOCK, LOW);
	delayMicroseconds(TIME);
	digitalWrite(RESET, HIGH);
	delayMicroseconds(TIME);
	digitalWrite(RESET, LOW);
	delayMicroseconds(TIME);
	digitalWrite(LOAD, LOW);

	delayMicroseconds(TIME / 2);

	for (int i = 0; i < (nr_of_rep*8) ; i++) {
/*	  if (mS88Debug) {
		  if (myBit==0) {  // in case we start a new 8 bit decoder
		    Serial.print("Decoder ");
		    Serial.println(myByte, HEX);
      }
      Serial.print(": bit ");
		  Serial.print(myBit, HEX);
		  Serial.print(" - ");
      Serial.println(digitalRead(DATA), HEX);
    }
*/
		bitWrite(mSwitches[myByte], myBit++, digitalRead(DATA));

		if (myBit == 8) {
    	if (mS88Debug) {
//	  	  Serial.println();
		    Serial.print("Readed back from decoder array ");
		    Serial.print(myByte, HEX);
		    Serial.print(": ");
 		    Serial.println(mSwitches[myByte], HEX);
 		    Serial.println();
 		  }
  		myByte++;  // next reporter (8 bit)
			myBit = 0;
			}
		digitalWrite(CLOCK, HIGH);
		delayMicroseconds(TIME);
		digitalWrite(CLOCK, LOW);
		delayMicroseconds(TIME / 2);
	}
}

byte TrackReporterS88::ProbeBus(int modules) {
	mSize = modules;
  boolean hit = false;
	int myByte = 0;
	int myBit = 0;
	int modulo = 0;

// override when called with param !=0
// in case called with <> then fixed reporter count is set	
	if (mSize != 0) {
  	if (mSize > 32) {
      mSize = 0;
    }
  	if (mS88Debug) {
	  	if (mSize = 0) {
        Serial.print("Error, requested > 32. ");
      }
      Serial.print("Number of S88 Reporters fixed to: ");
		  Serial.println(mSize, HEX);
    }
		mSize = 2 * mSize; // internal we use 8 bits
    return (mSize);
	}

	if (mS88Debug) {
		Serial.print("S88 Reporters requested: ");
		Serial.println(mSize, HEX);
		Serial.print("Max nr of S88 Reporters supported: ");
		Serial.println(MAX_REPORTER / 2, HEX);
	  Serial.println("Probing:");
	  Serial.println();
  }

// now we try to detect the number attached reporters
// read to the max

  readreporters(MAX_REPORTER);
	// reverse read back the buffer to detect the number of reporters
  if (mS88Debug) {
    Serial.println();
    Serial.println("Reading back buffer... ");
    for (mSize = MAX_REPORTER-1; mSize >= 0 ; mSize--) {
      Serial.print("Reporter nr: ");
      Serial.print(mSize, HEX);
      Serial.print(" ");
      Serial.println(mSwitches[mSize]);
    }
  }  
	for(mSize = MAX_REPORTER-1; mSize >= 0 ; mSize--) {
	  if (mSwitches[mSize] != 0xFF) {
      if (mS88Debug) {
        Serial.println();
        Serial.print("Reporter found in buffer ");
        Serial.print(mSize, HEX);
        Serial.print(" value: ");
        Serial.println(mSwitches[mSize], HEX);
      }
    	hit = true;
      break;	
		}
	}
  if (hit == false) {
    mSize=255;
    if (mS88Debug) {
      Serial.println("No reporters found!");
    }
  }
  if (hit) {
  // base is zero so compensation is needed for correct numbers
    mSize++;
	// compute nr of pairs
	  modulo = mSize % 2;

    if (mS88Debug) {
      Serial.print("mSize value: ");
      Serial.println(mSize, HEX);
 		  Serial.println();
      Serial.print("Modulo value: ");
      Serial.println(modulo, HEX);
    }
	  if (modulo = 1) {
		  mSize++;  //create even number for division
	  }

	  mSize = mSize / 2;
    
	  if (mS88Debug) {
		  Serial.print("nr of S88 (16 bit) reporters detected: ");
		  Serial.println(mSize);
	  }	
  }
// init the array

//	for (int i = 0; i <= sizeof(mSwitches); i++) {
//		mSwitches[i] = 0;
//	}
    return(mSize);
}

void TrackReporterS88::refresh() {
//	int myByte = 0;
//	int myBit = 0;
//	int k =1;
	int state = 0;

// copy to previous array
	if (mS88Debug) {
	  Serial.print("Reporters to copy: ");
	  Serial.println(mSize);
	}	

	for (int i = 0; i <= mSize*2; i++) {
		mPrevSwitches[i] = mSwitches[i];
	}

  readreporters(mSize*2);
//  checkDiff();
	refreshCount++;
  if (refreshCount == 128) {
    refreshCount=0;
    state=digitalRead(LED);
	  state = !state;
	  digitalWrite(LED, state);
	}
}

boolean TrackReporterS88::getValue(int index) {
//	index--;
	return bitRead(mSwitches[index / 8], index % 8);
}

boolean TrackReporterS88::getprevValue(int index) {
//	index--;
	return bitRead(mPrevSwitches[index / 8], index % 8);
}

boolean TrackReporterS88::setEventSwitches(int index) {
	return bitSet(mEventSwitches[index / 8], index % 8);
}

boolean TrackReporterS88::clrEventSwitches(int index) {
	return bitClear(mEventSwitches[index / 8], index % 8);
}

void TrackReporterS88::setEventReporters(int index) {
  for (int i=0; i<=index; i++) {
    mEventSwitches[i] = 0xff;
  }
}

byte TrackReporterS88::getDecoder(int index) {
  return (mSwitches[index]);
}

byte TrackReporterS88::getPrevDecoder(int index) {
  return (mPrevSwitches[index]);
}

